program molsem
  !
  ! ML/OS-EM Loop Handler
  !
  !
  ! LICENSE AGREEMENT:
  !
  ! You agree that we don't agree to anything.
  !
  !
  ! DISCLAIMER OF WARRANTIES:
  !
  ! This software is provided to you "AS IS," and the authors and
  ! contributors disclaim any warranty or liability obligations to you
  ! of any kind, implied or express, including but not limited to the
  ! warranties of merchantability, fitness for a particular purpose,
  ! and non-infringement.
  ! 
#define green(i) (iand(mana,i)/=0)
  use suty
  use ground_control
  use oct_pet
  use projectors
  implicit none
  integer k,is
  real loCj
  integer nbufi
  integer nbufj
  type t_image_buf
     real,pointer::buf(:,:)
     real,pointer::pad(:)
  end type t_image_buf
  type(t_image_buf),target::bxj
  type(t_image_buf),target::bCj
!!$  type(t_image_buf),target,allocatable::bCjs(:)
  type(t_image_buf),target::bDj
  real,allocatable,target::mrps_j(:,:,:,:)
  real,pointer::mrp_j_old(:,:,:)
  real,pointer::mrp_j(:,:,:)
  real,allocatable::m_j(:,:,:)
  real,pointer::x_j(:,:)  ! jr,jt,jz
  real,pointer::C_j(:,:)
  real,pointer::D_j(:,:)
  real,allocatable::uimg(:,:,:)
  real,allocatable::p_i(:,:,:,:,:,:)
  real,allocatable::y_i(:,:,:,:,:,:)
  real,allocatable::r_i(:,:,:,:,:,:)

  type t_cyln2rect_map
     integer n
     integer,allocatable::js(:)
     real,allocatable::ws(:)
  end type t_cyln2rect_map
  type(t_cyln2rect_map),allocatable::crmap(:,:)

  integer iarg,ifile,iarg_start
  character*512 fhist
  real yi_scale
  real::calib=1
  real retr
  integer nrun,irun

  type(t_cmd_info) ci

  call me_first_mana(ci)

  call setProg(-1)
  call initOctPet(ci%pet)
  call initProjectors(ci%pet)

  nbufi=mt*ma*mt*ma*npid*ngid
  allocate(p_i(mt,ma,mt,ma,npid,ngid))
  allocate(y_i(mt,ma,mt,ma,npid,ngid))
  allocate(r_i(mt,ma,mt,ma,npid,ngid))

  call allocImages
  if(green(OPT_USE_MRP)) then
     allocate(m_j(nx,ny,nz))
     allocate(mrps_j(nx,ny,nz,2))
     mrp_j_old => mrps_j(:,:,:,2)
     mrp_j     => mrps_j(:,:,:,1)
  else
     allocate(m_j(0,0,0))
  end if

  if(green(OPT_USE_SHM)) call initShm()
  call setCyln2rectMap

  !
  call setSysmat(ci%pet%num_multiray,ci%pet%num_line_sampling)
  p_i=1
  call disableSubsets
  !  call enableSubsets(.true.)
  !  do is=1,nos
  !     C_j => bCjs(is)%buf
  call bproj(p_i,C_j)
  !     call toNextSubset
  !  end do

  call scaleSysmat(1.0_dp/maxval(C_j))
  C_j = C_j / maxval(C_j)
  calib=1
  if(ci%fcj/="") call writeImageC(trim(ci%fcj),C_j) 

  loCj=max(maxval(C_j)*ci%pet%threshold_of_Cj,1.0e-9_dp)  
  call echo("loCj, thCj = "//trim(f2a(loCj,-4))//", "//trim(f2a(ci%pet%threshold_of_Cj,-4)))
  call initSuty(1.0e-9_dp,loCj)

  if(green(OPT_BATCH)) then
     nrun = ci%nfin
  else
     nrun = 1
  end if
  
  calib=ci%pet%cf
  if(green(OPT_DECAY_CORRECTION)) then
     retr=getDecayCorrectionFactor()
     if(retr<=0) call echo_red("Failed to obtain decay correction factor")
     call echo("Decay correction factor: "//trim(f2a(retr,4)))
     calib=retr
  end if
  if(green(OPT_CALIBRATION)) then
     retr=getCalibrationFactor()
     if(retr<=0) call echo_red("Failed to obtain calibration factor")
     call echo("Dose/Weight calibration factor: "//trim(f2a(retr,4)))
     calib=calib*retr
  end if
  call echo("Calibration factor: "//trim(f2a(calib,4)))

  iarg_start = 1
  ifile = 0
  file: do irun=1, nrun
     
     y_i=0
     r_i=0
     do iarg=iarg_start,ci%narg
        if(ci%argindxfile(iarg)==0) cycle
        ifile=ifile+1
        call get_command_argument(iarg,fhist)
        select case(ci%argindxfile(iarg))
        case(FT_HIST)
           call readHist(fhist,y_i)
           if(green(OPT_BATCH)) exit
        case(FT_LIST)
           call readList(fhist,y_i,r_i)
           if(green(OPT_BATCH)) exit
        end select
     end do
     
     if(associated(ci%pet%fhp)) call writeHist(y_i,ci%pet%fhp)
     if(associated(ci%pet%fhd)) call writeHist(r_i,ci%pet%fhd)

     if(associated(ci%pet%fht).or.green(OPT_USE_TRUE))then
        where(y_i>r_i)
           r_i=y_i-r_i
        elsewhere
           r_i=0
        end where
        if(associated(ci%pet%fht)) call writeHist(y_i,ci%pet%fht)
        if(green(OPT_USE_TRUE)) y_i=r_i
     end if

     if(green(OPT_BATCH)) then
        ci%fout=fhist
        iarg_start = iarg + 1
     end if

     yi_scale=sum(y_i)
     if(yi_scale==0) call echo_red("*** histogram is empty!")
     x_j=yi_scale/sum(C_j) ! normalize by sysmat
     
     call enableSubsets
     main: do k=1,ci%pet%number_of_iteration
        call echo("Begin iteration "//trim(i2a(k))//" of "//trim(i2a(ci%pet%number_of_iteration)),.true.)
        call setProg(k)
        
        subset: do is = 1,nos
           if(nos>1) call echo("Begin subset "//trim(i2a(is))//" of "//trim(i2a(nos)),.true.)
           
           ! p_i = \sum_j C_ij x_j
           call fproj(x_j,p_i)
           call echo_v(nbufi,loc(p_i),"p_i")
           if(green(OPT_SHOW_LOGL)) call echo("LogL = "//trim(f2a(getLogL())))
           
           ! p_i = y_i / p_i
           call divpd(loc(p_i),loc(y_i),nbufi)
           call echo_v(nbufi,loc(p_i),"q_i")
           
           ! D_j = \sum_i C_ij p_i
           call bproj(p_i,D_j)
           call echo_v(nbufj,loc(d_j),"D_j")       
           
           ! x_j = D_j/C_j * x_j  
           call muldivpd(loc(x_j),loc(D_j),loc(C_j),nbufj)
           call echo_v(nbufj,loc(x_j),"x_j")
           
           call toNextSubset
           
        end do subset
        
        if(green(OPT_USE_MRP)) then
           if(k==1) then
              call cyln2rect(x_j,mrp_j_old)
              cycle
           end if
           call cyln2rect(x_j,mrp_j)
           call getMedian(mrp_j_old,m_j)
           call echo_v(nx*ny*nz,loc(m_j),"m_j")
           where(m_j>0) 
              mrp_j=mrp_j/(1.+ci%pet%beta*(mrp_j_old/m_j-1.))
           elsewhere
              mrp_j=0
           end where
           mrp_j_old=mrp_j
        end if
        
        if(green(OPT_WRITE_EACH)) then
           if(green(OPT_USE_MRP)) then
              call writeImage(ci%fout,mrp_j,k)
           else
              call writeImageC(ci%fout,x_j,k)
           end if
        end if
        
     end do main
     
     if(.not.green(OPT_WRITE_EACH)) then
        if(green(OPT_USE_MRP)) then
           call writeImage(ci%fout,mrp_j)
        else
           call writeImageC(ci%fout,x_j)
        end if
     end if

  end do file
  call setProg(0) 
  
  call exit(0)
  
contains

  subroutine allocImages    
!!$    integer is
    allocate(bxj%buf(nj,nz))
    allocate(bDj%buf(nj,nz))
    allocate(bCj%buf(nj,nz))
!!$    allocate(bCjs(nos))
!!$    do is=1,nos
!!$       allocate(bCjs(is)%buf(nj,nz))
!!$    end do
    
    x_j => bxj%buf
    C_j => bCj%buf
    D_j => bDj%buf
    x_j=0
    C_j=0
    D_j=0
    nbufj=nj*nz
    if(iand(nbufj,1)==1) then
       allocate(bxj%pad(1))
       allocate(bCj%pad(1))
       allocate(bDj%pad(1))
!!$       do is=1,nos
!!$          allocate(bCjs(is)%pad(1))
!!$       end do        
       nbufj=nbufj+1
    end if
  end subroutine allocImages

  subroutine echo(mess,put_time)
    use suty
    character*(*),intent(in)::mess
    logical,intent(in),optional::put_time
    if(.not.green(OPT_VERBOSE)) return
    call echo_green(mess,"","ml",put_time)
  end subroutine echo

  subroutine setCyln2rectMap
    integer jr,jt
    real x,y,fx,fy
    integer jx,jy
    integer j
    integer c(nx,ny)
    real w

    allocate(crmap(nx,ny))
    crmap%n=0
    c = 0

    do jr=0,nr
       do jt=1,getnt(jr)
          j=jrt2j(jr,jt)             
          call jrt2xy(jr,jt,x,y)
          call x2jx(x,jx,fx)
          call y2jy(y,jy,fy)
          if((1-fx)*(1-fy)>0) crmap(jx,jy)%n=crmap(jx,jy)%n+1
          if(jx<nx.and.fx*(1-fy)>0) crmap(jx+1,jy)%n=crmap(jx+1,jy)%n+1
          if(jy<ny.and.fy*(1-fx)>0) crmap(jx,jy+1)%n=crmap(jx,jy+1)%n+1
          if(jx<nx.and.jy<ny.and.fx*fy>0) crmap(jx+1,jy+1)%n=crmap(jx+1,jy+1)%n+1
       end do
    end do

    do jy=1,ny
       do jx=1,nx
          if(crmap(jx,jy)%n==0) cycle
          allocate(crmap(jx,jy)%js(crmap(jx,jy)%n))
          allocate(crmap(jx,jy)%ws(crmap(jx,jy)%n))          
       end do
    end do

    do jr=0,nr
       do jt=1,getnt(jr)
          j=jrt2j(jr,jt)             
          call jrt2xy(jr,jt,x,y)
          call x2jx(x,jx,fx)
          call y2jy(y,jy,fy)
          
          w=(1-fx)*(1-fy)
          if(w>0) then
             c(jx,jy)=c(jx,jy)+1
             crmap(jx,jy)%js(c(jx,jy))=j
             crmap(jx,jy)%ws(c(jx,jy))=w
          end if

          w=fx*(1-fy)
          if(jx<nx.and.w>0) then
             c(jx+1,jy)=c(jx+1,jy)+1
             crmap(jx+1,jy)%js(c(jx+1,jy))=j
             crmap(jx+1,jy)%ws(c(jx+1,jy))=w
          end if

          w=(1-fx)*fy
          if(jy<ny.and.w>0) then
             c(jx,jy+1)=c(jx,jy+1)+1
             crmap(jx,jy+1)%js(c(jx,jy+1))=j
             crmap(jx,jy+1)%ws(c(jx,jy+1))=w
          end if

          w=fx*fy
          if(jx<nx.and.jy<ny.and.w>0) then
             c(jx+1,jy+1)=c(jx+1,jy+1)+1
             crmap(jx+1,jy+1)%js(c(jx+1,jy+1))=j
             crmap(jx+1,jy+1)%ws(c(jx+1,jy+1))=w
          end if
       end do
    end do

  end subroutine setCyln2rectMap

  recursive subroutine cyln2rect(cimg,rimg,num_slice)
    real,intent(in)::cimg(:,:)
    real,intent(out)::rimg(:,:,:)
    integer,intent(in),optional::num_slice
    integer jz,jr,jt
    real x,y,fx,fy
    integer jx,jy
    integer j
    real v
    real,allocatable::timg(:,:)
    integer ns

    if(present(num_slice)) then
       ns=num_slice
    else
       ns=nz
    end if

    if(.not.allocated(uimg)) then
       allocate(uimg(nx,ny,1))
       uimg=0
       allocate(timg(nj,nz))
       timg=1
       call cyln2rect(timg,uimg,1)
       deallocate(timg)
    end if

    do jz=1,ns
       j=0
       do jr=0,nr
          do jt=1,getnt(jr)
             j=j+1
             v=cimg(j,jz)
             if(v==0) cycle
             call jrt2xy(jr,jt,x,y)
             call x2jx(x,jx,fx)
             call y2jy(y,jy,fy)
             rimg(jx,   jy,   jz) = rimg(jx,   jy,   jz) + v*(1-fx)*(1-fy)
             if(jx<nx)&
                  rimg(jx+1, jy,   jz) = rimg(jx+1, jy,   jz) + v*fx*(1-fy)
             if(jy<ny)&
                  rimg(jx,   jy+1, jz) = rimg(jx,   jy+1, jz) + v*(1-fx)*fy
             if(jx<nx.and.jy<ny)&
                  rimg(jx+1, jy+1, jz) = rimg(jx+1, jy+1, jz) + v*fx*fy
          end do
       end do
    end do
    
    do jy=1,ny
       do jx=1,ny
          if(uimg(jx,jy,1)>0) then
             rimg(jx,jy,:)=rimg(jx,jy,:)/uimg(jx,jy,1)
          else
             rimg(jx,jy,:)=0
          end if
       end do
    end do
  end subroutine cyln2rect

  subroutine getMedian(rimg,mimg)
    real,intent(in)::rimg(:,:,:)
    real,intent(out)::mimg(:,:,:)
    integer jx,jy,jz
    real v(9)
    do jz=1,nz
       do jy=2,ny-1
          do jx=2,nx-1
             v(1:3)=rimg(jx-1:jx+1,jy-1,jz)
             v(4:6)=rimg(jx-1:jx+1,jy,  jz)
             v(7:9)=rimg(jx-1:jx+1,jy+1,jz)
             mimg(jx,jy,jz)=median(9,v)
          end do
       end do
    end do
  end subroutine getMedian

  subroutine writeImage(f,imgin,k,j)
    use duty
    use auty
    use suty
    character*(*),intent(in)::f
    real,intent(in)::imgin(:,:,:)
    integer,intent(in),optional::k,j
    character(MAXPATHLEN) fout,fhdr
    real img(nx,ny,nz)
    integer*2 wimg(nx,ny,nz)
    real maxv,norm
    type(image_info) ifo
    type(ana_hdr) hdr
    integer,parameter::int_max = 16383
    character*128 str
    integer istat

    img=imgin*calib

    if(green(OPT_USE_SHM)) then
       call writeImageShm(loc(img),nx*ny*nz,nx,ny,nz,dx,dy,dz)
    end if
    
    call proc_output(f,fout,ci%oext,k,j)
    open(unit=1,file=fout,access="stream",iostat=istat,status="replace")
    if(istat/=0) call echo_red( "*** Error during open: " //trim(fout))

    select case(ci%ofmtid)
    case(OFMT_RAW)
       write(1,iostat=istat) img
    case(OFMT_ANA)
       call clemem1(loc(hdr),sizeof(hdr))
       hdr%hk%data_type="dsr"
       hdr%hk%db_name=trim(fout)
       hdr%hk%sizeof_hdr=348_4
       hdr%hk%extents=16384_4
       hdr%hk%regular="r"
       hdr%id%dim(1)=4_2
       hdr%id%dim(2)=int(nx,2)
       hdr%id%dim(3)=int(ny,2)
       hdr%id%dim(4)=int(nz,2)
       hdr%id%dim(5)=1_2
       hdr%id%pixdim(1)=4_4
       hdr%id%pixdim(2)=real(dx,4)
       hdr%id%pixdim(3)=real(dy,4)
       hdr%id%pixdim(4)=real(dz,4)
       hdr%id%vox_units="mm"
       hdr%id%cal_units="count"
       hdr%id%datatype=int(DT_DOUBLE,2)
       hdr%id%bitpix=64_2
       hdr%id%cal_max=1_4
       hdr%id%glmax=1_4
       call getenv("RP3STUDYDESC",hdr%dh%descrip)
       call getenv("RP3DERICDESC",hdr%dh%generated)
       hdr%dh%originator="meliy"
       call getenv("RP3SID",hdr%dh%scannum)
       call getenv("RP3PID",hdr%dh%patient_id)
       call getenv("RP3SYEAR",str)
       hdr%dh%exp_date=trim(str)
       call getenv("RP3SMONTH",str)
       hdr%dh%exp_date=trim(hdr%dh%exp_date)//trim(str)
       call getenv("RP3SDAY",str)
       hdr%dh%exp_date=trim(hdr%dh%exp_date)//trim(str)
       call getenv("RP3SHOUR",str)
       hdr%dh%exp_time=trim(str)
       call getenv("RP3SMINUTE",str)
       hdr%dh%exp_time=trim(hdr%dh%exp_time)//trim(str)
       call getenv("RP3SSECOND",str)
       hdr%dh%exp_time=trim(hdr%dh%exp_time)//trim(str)       
       call proc_output(f,fhdr,".hdr",k,j,.true.)
       open(unit=2,file=fhdr,access="stream",iostat=istat,status="replace")
       if(istat/=0) call echo_red( "*** Error during open: " //trim(fhdr))
       write(2,iostat=istat) hdr
       if(istat/=0) call echo_red( "*** Error during write: " //trim(fhdr))
       close(2)
       write(1,iostat=istat) img
    case(OFMT_DCM)
       call clemem1(loc(ifo),sizeof(ifo))
       call getenv("RP3PID",ifo%pinf%id)
       call getenv("RP3PNAME",ifo%pinf%name)
       call getenv("RP3PDOB",ifo%pinf%dob)
       call getenv("RP3PSEX",ifo%pinf%sex)
       call getenv("RP3PAGE",str)
       ifo%pinf%age=a2i(str)
       call getenv("RP3PWEIGHT",str)
       ifo%pinf%weight=a2i(str)
       call getenv("RP3PHEIGHT",str)
       ifo%pinf%height=a2i(str)
       !
       call getenv("RP3SID",ifo%sinf%id)
       ifo%sinf%input=trim(ifo%sinf%id)
       ifo%sinf%ofile=trim(fout)
       call getenv("RP3SYEAR",str)
       ifo%sinf%year=a2i(str)
       call getenv("RP3SMONTH",str)
       ifo%sinf%month=a2i(str)
       call getenv("RP3SDAY",str)
       ifo%sinf%day=a2i(str)
       call getenv("RP3SHOUR",str)
       ifo%sinf%hour=a2i(str)
       call getenv("RP3SMINUTE",str)
       ifo%sinf%minute=a2i(str)
       call getenv("RP3SSECOND",str)
       ifo%sinf%second=a2i(str)
       
       call getenv("RP3SPART",ifo%sinf%part)
       call getenv("RP3STUDYDESC",ifo%sinf%description)
       if(len_trim(ifo%sinf%part)>0.and.len_trim(ifo%sinf%description)>0)&
            ifo%sinf%description=trim(ifo%sinf%description)//" "//trim(ifo%sinf%part)
       call getenv("RP3DERIVDESC",ifo%einf%derivDesc)
       call getenv("RP3SINT",ifo%sinf%injectionTime)
       call getenv("RP3SDOSE",str)
       ifo%sinf%dose=a2f(str)
       call getenv("RP3RPHAR",ifo%sinf%rphar)
       call getenv("RP3HALFLIFE",str)
       ifo%sinf%halflife=a2f(str)*60 ! in sec
       call getenv("RP3PFRAC",str)
       ifo%sinf%positronFrac=a2f(str)
       call getenv("RP3SSCAND",str)
       ifo%sinf%duration=a2i(str)
       !
       call getenv("RP3INSTITUTION",ifo%einf%institution)
       call getenv("RP3OPERATOR",ifo%einf%operator)
       
       ifo%slice_width=dz
       ifo%slice_separation=dz
       ifo%number_of_slices=nz
       ifo%width=int(nx,2)
       ifo%height=int(ny,2)
       ifo%dx=dx
       ifo%dy=dy

       maxv=maxval(img)
       if(maxv>0) norm=int_max/maxv
       wimg=int(img*norm+0.5_dp,kind=2)
       call echo("Dicom integerized by: "//trim(f2a(norm,-1)))
       ifo%intcpt=0
       ifo%bias=1.0_dp/norm
       istat=write_dcm(1,ifo,loc(wimg),nx*ny*nz)
    end select
    close(1)
    if(istat/=0) call echo_red( "*** Error during write: "//trim(fout))
    call echo("Image written: "//trim(fout))
  end subroutine writeImage


  subroutine writeImageC(f,img,k,j)
    character*(*),intent(in)::f
    real,intent(in)::img(:,:)
    integer,intent(in),optional::k,j
    real rimg(nx,ny,nz)
    rimg=0
    call cyln2rect(img,rimg)
    call writeImage(f,rimg,k,j)    
  end subroutine writeImageC

  real function getLogL()
    real v(mt,ma,mt,ma,npid,ngid)
    v=p_i
    where(v>0)
       v=-(v)+y_i*log(v)
    elsewhere
       v=0
    end where
    getlogL=sum(v)
  end function getLogL

  real function getDecayCorrectionFactor()
    character*128 str
    real t0,t1,dt,tau,x
    getDecayCorrectionFactor=-1
    call getenv("RP3SINT",str)
    if(len_trim(str)/=5) return
    t0=(a2f(str(1:2))*60+a2f(str(4:5)))*60

    call getenv("RP3SMINUTE",str)
    if(len_trim(str)/=2) return
    t1=a2f(str)*60

    call getenv("RP3SHOUR",str)
    if(len_trim(str)/=2) return
    t1=t1+a2f(str)*60*60-t0

    call getenv("RP3SSCAND",str) ! in sec
    if(str=="") return
    dt=a2f(str)

    call getenv("RP3HALFLIFE",str) ! in min
    if(str=="") return
    tau=a2f(str)*60/log(2.0_dp)

    x=dt/tau
    getDecayCorrectionFactor=x*exp(t1/tau)/(1.0_dp-exp(-x))

  end function getDecayCorrectionFactor

  real function getCalibrationFactor()
    character*128 str
    real w,d,p
    getCalibrationFactor=-1
    
    call getenv("RP3PWEIGHT",str) ! kg
    if(str=="") return
    w=a2f(str)

    call getenv("RP3SDOSE",str) ! Bq
    if(str=="") return
    d=a2f(str)/3.7e10_dp*1.0e3_dp ! mCi
    
    p=1
    call getenv("RP3PFRAC",str)
    if(str/="") then
       p=a2f(str)
    else
       call echo("*** getCalibrationFactor: RP3PFRAC not set, assuming Positron fraction = 1.0")
    end if

    getCalibrationFactor=p/(d/w)

  end function getCalibrationFactor
  
end program molsem
